#include <cmath>
#include <iostream>
#include <fstream>
#include <vector> 
#include <algorithm>
#include <iterator>
#include <numeric>
#include <limits>
#include <valarray>
#include <string>
#include <sstream>

#include "bellman.h"
#include "struct.h"
#include "int2str.h"
#include "is_eq.h"
#include "zadacha.h"


using namespace std;

typedef valarray<double> vad;

valarray<double> bellman_init (input& data, string intxt, double &delta, valarray<double> w_oc, bool seek_oc, double step) {
  unsigned n = data.m;              //размерность системы
  unsigned num_upr = data.num_upr ; //число управлений

  double tf = data.tf;
  valarray<double> c(num_upr);       //число моментов переключений, одинаковое для всех управлений
  c = data.c;
  unsigned p = data.n;              //общее число параметров управления w
  vector<vector<double> > tau = data.tau; //таблица моментов переключений по каждому управлению
  valarray<double> x(n);
  vector<double> w(p), w_gr(2*p);
  w_gr = data.rgn;
  valarray<double> x0 = data.x;
  valarray<double> _w(p);

  if (seek_oc == 1) {
    for (unsigned i=0; i<w_oc.size(); i++) {
      if (w_oc[i] < w_gr[2*i]) w_gr[2*i] = w_oc[i];
      if (w_oc[i] > w_gr[2*i+1]) w_gr[2*i+1] = w_oc[i];
    }
  }
  double phi;
  double F_oc = 0;

  vector<double> w_beg(p), w_fin(p), w_step(p);
  for (unsigned i=0; i<w.size(); i++) {
    w_step[i] = step;
    w_beg[i] = w_gr[2*i];
    w_fin[i] = w_gr[2*i+1];
  }

  unsigned num = data.num;                    //считаем, что число точек переключения одинаковое число
  valarray<double> uu(num);                  //!!!!!!!!
  double tt;
  

  vector<unsigned> sum(num_upr+1);
  sum = data.sum;

  vector<double> upr, tra_oc, tra, u(2*num_upr), res_uu;

  unsigned s = data.s;

  //схема Беллмана
  //--------------------------------------------------------------------------   
  if (seek_oc == 1) {
    for (unsigned i=0; i<w.size(); i++) w[i] = w_oc[i];
    for (unsigned i=0; i<num_upr; i++) {
      uu = vad(w_oc[slice(s*sum[i],uu.size(),1)]);
      for (unsigned i=0; i<uu.size(); i++) res_uu.push_back(uu[i]);
    }

    //    x = x0;
    for (unsigned i=0; i<x.size(); i++) x[i] = x0[i];


    for (unsigned i=0; i<x.size(); i++) tra_oc.push_back(x[i]);
    for (unsigned i=0; i<u.size(); i++) u[i] = res_uu[i];

    for (unsigned j=0; !IsEq(tau[0][j],tf,1e-11); j++) {
      tt = tau[0][j];
      for (unsigned i=0; i<u.size(); i++) u[i] = res_uu[i];
      x = mu(tt,tau[0][j+1],x,u,data);
      for (unsigned i=0; i<x.size(); i++) tra_oc.push_back(x[i]);
      res_uu.erase(res_uu.begin(),res_uu.begin()+s*num_upr);
    }
    F_oc = F0(x,data);
    phi = -F0(x,data);
  }
  vector<double> w0(p), w1(p);
  w1 = w_beg;
  w0 = w_beg;
  
  w0.resize(s);
  w1.resize(s);

  int from = w0.size(); //outside
  int to = 0;           //outside  

  int nodes = 0;
  while (w1.size()>0) {
    w1 = next_w (w1.size(), from, to, w1, w_beg, w_fin, w_step);
    nodes++;
  }
  //cout << "nodes= " << nodes << endl;
  
  tval struct oresults ores[nodes];
  struct files names[nodes];
  
  string file = intxt;
  string file_name_in = "in";
  string file_name_out = "out";


  int k = 0;
  while (w0.size()>0) {
    string s_in(file_name_in + Int2Str(k) + ".txt");
    string s_out(file_name_out + Int2Str(k) + ".txt");
    string s(file);
    strcpy(names[k].in_file, s_in.c_str());
    strcpy(names[k].out_file, s_out.c_str());
    strcpy(names[k].file, s.c_str());
    ofstream out(s_in.c_str(), ios::trunc);
    out << data.tip_metoda << endl;
    out << data.tip_integr << endl;
    out << step << endl;
    for (unsigned i=0; i<w0.size(); i++) out << w0[i] << " ";
    out.close();
    w0 = next_w (w0.size(), from, to, w0, w_beg, w_fin, w_step);
    k++;
  }


  for (int i=0; i<k; i++) {
    tct(atRank (i % ts::realsuperSize));
    bellman(names[i],ores[i]);
  }


  for (int i=0; i<k; i++) {
    struct oresults res = (struct oresults) ores[i];
  }
  

  //-----------------------------------------------------------------------------------------
  std::remove("result.txt");
  string result = "result.txt";
  ofstream res(result.c_str(), ios::app);

  if (seek_oc == 1) print_result(res,tra_oc,w,phi);

  string str;

  double val;

  vector<double> numbs; vector<double> phi_v;
  
  if (seek_oc == 1) phi_v.push_back(phi);

  for (int i=0; i<k; i++) {
    string s_from(file_name_out + Int2Str(i) + ".txt");
    ifstream from(s_from.c_str());
    while (getline(from,str)) {
      res << str << endl;
      istringstream ss(str);  
      while (ss >> val) numbs.push_back(val);
      phi_v.push_back(numbs[numbs.size()-1]);
    }
  }  

  vector<double>::iterator iter_max = (max_element(phi_v.begin(), phi_v.end()));
  unsigned pos = distance(phi_v.begin(),iter_max);

  ifstream opt(result.c_str());
  for (unsigned i=0; i<=pos; i++) getline(opt,str);

  stringstream ss(str);
  unsigned len = n*(c[0]+2); //длина tra
  unsigned m = /*tra_oc.size()*/ len + w.size();
  vector<double> vec(m);
  vector<double> w_res;
  for (unsigned i=0; i<m; i++) ss >> vec[i]; 
  for (unsigned i=len; i<m; i++) w_res.push_back(vec[i]);
  //for (unsigned i=0; i<w_res.size(); i++) cout << w_res[i] << " "; cout << endl;
  //----------------------------------------------------------------------------------------------------------

  delta =  F_oc + *iter_max;  
  //cout << "delta= " << F_oc << " + " << *iter_max << " = "  << F_oc + *iter_max << endl;

  
  unsigned l = w_res.size();
  valarray<double> appr0(l);
  for (unsigned i=0; i<l; i++) appr0[i] = w_res[i]; 
  
  return appr0;
}



tfun int bellman (struct files names, tout struct oresults ores) {
  ifstream in (names.in_file);
  ofstream out (names.out_file);
  unsigned s, tip_upr, tip_integr;
  double step;

  in >> tip_upr;
  in >> tip_integr;
  in >> step;

  input data;
  data.set_method(tip_upr,tip_integr);
  data.init(names.file);

  data.tip_metoda = tip_upr;
  data.tip_integr = tip_integr;
  
  s = data.s;
  vector<double> w0(s);
  for (unsigned i=0; i<w0.size(); i++) in >> w0[i];


  struct oresults res;
  //cout << "Node= " << ts::myRank << endl; 
          
  unsigned n = data.m;      //размерность системы
  //cout << "n= " << n << endl;

  unsigned num_upr = data.num_upr; //число управлений
  double tf = data.tf;

  valarray<double> c(num_upr); // число моментов переключений, одинаковое для всех управлений
  c = data.c;  
  unsigned p = data.n;

  //таблица моментов переключений по каждому управлению
  vector<vector<double> > tau = data.tau;

  valarray<double> x(n), x0(n);
  vector<double> w(p), w_gr(2*p);
  w_gr = data.rgn;
  x0 = data.x;

  valarray<double> _w(p), w_oc(p);

  unsigned num = data.num; //считаем, что число точек переключения одинаковое число
  valarray<double> uu(num);                  //!!!!!!!!
  double tt;


  vector<unsigned> sum(num_upr+1);
  sum = data.sum;

  vector<double> upr, tra_oc, tra, u(2*num_upr), res_uu;

  vector<double> w_beg(p), w_fin(p), w_step(p);
  for (unsigned i=0; i<w.size(); i++) {
    w_step[i] = step;
    w_beg[i] = w_gr[2*i];
    w_fin[i] = w_gr[2*i+1];
  }


  double phi;
  int from = w.size();
  int to = s;

  w = w_beg;
  for (unsigned i=0; i<w0.size(); i++) w[i] = w0[i];

  while (w.size()>0) {
    for (unsigned i=0; i<w.size(); i++) _w[i] = w[i];
    for (unsigned i=0; i<num_upr; i++) {
      uu = vad(_w[slice(s*sum[i],uu.size(),1)]);
      for (unsigned i=0; i<uu.size(); i++) res_uu.push_back(uu[i]);
    }

    //    x = x0;
    for (unsigned i=0; i<x.size(); i++) x[i] = x0[i];


    for (unsigned i=0; i<x.size(); i++) tra.push_back(x[i]);
    for (unsigned i=0; i<u.size(); i++) u[i] = res_uu[i];


    for (unsigned j=0; !IsEq(tau[0][j],tf,1e-11); j++) {
      tt = tau[0][j];
      for (unsigned i=0; i<u.size(); i++) u[i] = res_uu[i];
      x =  mu(tt,tau[0][j+1],x,u,data);
      for (unsigned i=0; i<x.size(); i++) tra.push_back(x[i]);
      res_uu.erase(res_uu.begin(),res_uu.begin()+s*num_upr);
    }
    phi = -F0(x,data);
    print_result(out,tra,w,phi);

    tra.clear();
    w = next_w (w.size(), from, to, w, w_beg, w_fin, w_step);
  }

  
  ores = (struct oresults) res;
  return 1;
}


valarray<double> mu (double t0, double t1, valarray<double> x, vector<double> u, input &data) {
  unsigned N = data.N;
  double th = fabs(t1 - t0)/N;

  double t = t0;

  for (unsigned i=0; i<N; i++) {
    rk4 (t, t0, t1, th, &x[0], u, data);
    t += th;
  }
  return x;
}


void rk4 (double t, double tI, double tF, double th, double* x, vector<double> upr, input &data) {
  unsigned q = data.q;
  valarray<double> k1(q),k2(q),k3(q),k4(q),dydx(q),xd(q),rez(q);

  unsigned k = upr.size();
  valarray<double> u(k);
  for (unsigned i=0; i<k; i++) u[i] = upr[i]; 

  unsigned num_upr = data.num_upr;

  if ((data.tip_metoda == 2) || (data.tip_metoda == 3)) for (unsigned i=0; i<num_upr; i++) u[i] = ((u[2*i+1]-u[2*i])*t-(tI*u[2*i+1]-tF*u[2*i]))/(tF-tI);

  for (unsigned i=0; i<data.m; i++) rez[i] = x[i];

  for (unsigned i=0; i<q; i++) xd[i] = rez[i];
  dydx = right_parts (xd,t,u,data);
  for (unsigned i=0; i<q; i++) k1[i] = th*dydx[i];

  for (unsigned i=0; i<q; i++) xd[i] = rez[i]+k1[i]/2;
  dydx = right_parts (xd,t+th/2,u,data);
  for (unsigned i=0; i<q; i++) k2[i] = th*dydx[i];

  for (unsigned i=0; i<q; i++) xd[i] = rez[i]+k2[i]/2;
  dydx = right_parts (xd,t+th/2,u,data);
  for (unsigned i=0; i<q; i++) k3[i] = th*dydx[i];

  for (unsigned i=0; i<q; i++) xd[i] = rez[i]+k3[i];
  dydx = right_parts (xd,t+th,u,data);
  for (unsigned i=0; i<q; i++) k4[i] = th*dydx[i];

  for (unsigned i=0; i<q; i++) {
    rez[i] += k1[i]/6 + k2[i]/3 + k3[i]/3 + k4[i]/6;
  }

  for (unsigned i=0; i<data.m; i++) x[i] = rez[i];
}


void print_result (ofstream &itog, vector<double> tra, vector<double> w, double phi) {
  for (unsigned i=0; i<tra.size(); i++) itog << tra[i] << " ";
  /* itog << setw(4); */ itog << "     ";
  for (unsigned i=0; i<w.size(); i++) itog << w[i] << " ";
  /* itog << setw(4); */ itog << "     ";
  itog << phi << " ";
  itog << endl;
}


vector<double> next_w (int n, int from, int to, vector<double> u_cur, vector<double> u_beg,
                       vector<double> u_fin, vector<double> u_step) {
  vector<double> exit;
  u_cur.resize(n); u_beg.resize(n); u_fin.resize(n); u_step.resize(n);
  for (int k=from-1; k>=to; k--) {
    if ((u_step[k]!=0) && ((u_cur[k] < u_fin[k]-u_step[k]) || (IsEq(u_cur[k], u_fin[k]-u_step[k], 1e-11)))) {
      u_cur[k] += u_step[k];
      return u_cur;
    } else u_cur[k] = u_beg[k];
  }
  return exit;
}
